function plot_grid_slam(true_pose, particles, occ_map, obstacles, true_traj, est_traj, ...
                        scan, max_range, fov, num_beams, resolution, map_size, time)
% 占据栅格SLAM可视化

    clf;
    
    % 子图1: 占据栅格地图
    subplot(1, 2, 1);
    hold on; axis equal;
    
    % 显示占据栅格（转换为概率）
    prob_map = 1 - 1./(1 + exp(occ_map));  % log-odds转概率
    imagesc([0, map_size], [0, map_size], prob_map);  % 直接显示，occ_map(y,x)格式
    colormap(flipud(gray));
    caxis([0, 1]);
    axis xy;  % 设置坐标轴方向：x向右，y向上
    
    % 镜像变换轨迹坐标（以y=125为轴）
    mirror_y = @(y) 250 - y;
    
    % 真实轨迹
    plot(true_traj(1, :), mirror_y(true_traj(2, :)), 'g-', 'LineWidth', 2);
    
    % 估计轨迹
    plot(est_traj(1, :), mirror_y(est_traj(2, :)), 'b--', 'LineWidth', 1.5);
    
    % 真实位置
    draw_robot(true_pose(1), mirror_y(true_pose(2)), true_pose(3), 0.8, 'g');
    
    % 粒子
    for i = 1:length(particles)
        pose = particles(i).pose;
        w = particles(i).weight;
        markersize = max(1, w * length(particles) * 20);
        plot(pose(1), mirror_y(pose(2)), 'b.', 'MarkerSize', markersize);
    end
    
    xlabel('X(m)'); ylabel('Y(m)');
    title(sprintf('占据栅格地图 (t=%.1fs)', time));
    xlim([0, map_size]); ylim([0, map_size]);
    grid on;
    hold off;
    
    % 子图2: 激光扫描可视化
    subplot(1, 2, 2);
    hold on; axis equal;
    
    % 真实障碍物（带颜色区分）
    for i = 1:size(obstacles, 1)
        obs = obstacles(i, :);
        if i <= 4
            % 外墙用深色
            rectangle('Position', [obs(1), obs(2), obs(3)-obs(1), obs(4)-obs(2)], ...
                      'FaceColor', [0.3, 0.3, 0.3], 'EdgeColor', 'k', 'LineWidth', 1.5);
        else
            % 内部障碍物用浅色
            rectangle('Position', [obs(1), obs(2), obs(3)-obs(1), obs(4)-obs(2)], ...
                      'FaceColor', [0.7, 0.7, 0.7], 'EdgeColor', 'k');
        end
    end
    
    % 真实轨迹（不进行镜像变换，保持原样）
    plot(true_traj(1, :), true_traj(2, :), 'g-', 'LineWidth', 1.5);
    
    % 真实机器人（不进行镜像变换，保持原样）
    draw_robot(true_pose(1), true_pose(2), true_pose(3), 0.8, 'g');
    
    % 激光扫描点（不进行镜像变换，保持原样）
    theta = true_pose(3);
    angle_min = theta - fov/2;
    angle_max = theta + fov/2;
    angles = linspace(angle_min, angle_max, num_beams);
    
    scan_x = true_pose(1) + scan .* cos(angles');
    scan_y = true_pose(2) + scan .* sin(angles');
    plot(scan_x, scan_y, 'r.', 'MarkerSize', 8);
    
    % 激光束
    for i = 1:5:num_beams
        if scan(i) < max_range
            plot([true_pose(1), scan_x(i)], [true_pose(2), scan_y(i)], 'r-', 'LineWidth', 0.5);
        end
    end
    
    xlabel('X(m)'); ylabel('Y(m)');
    title(sprintf('激光扫描 (粒子:%d)', length(particles)));
    xlim([0, map_size]); ylim([0, map_size]);
    grid on;
    hold off;
end

function draw_robot(x, y, theta, size, color)
    robot_points = size * [0.5, 0; -0.5, 0.3; -0.5, -0.3; 0.5, 0];
    R = [cos(theta), -sin(theta); sin(theta), cos(theta)];
    robot_points = (R * robot_points')';
    robot_points(:, 1) = robot_points(:, 1) + x;
    robot_points(:, 2) = robot_points(:, 2) + y;
    plot(robot_points(:, 1), robot_points(:, 2), color, 'LineWidth', 2);
end

